Motion 范例

此范例程式说明如何透过 Win32 应用程式使用 KINGSTAR 子系统及介绍 API 函式。在此范例中，Win32 指的是 Windows API，而非 32 位元，KINGSTAR Win32 API 与 KINGSTAR RT API 相同，使用 Win32 介面时，不需了解 RTX64 和即时开发，因开启与停止子系统将会自动开启和停止即时环境。

因即时和非即时之间的连接为异步的，且 Windows 非即时环境，所有对子系统的呼叫都将伫列， 因此应用程式无法有非常短 (<5 ms) 的更新周期，若想要更快的更新周期，请考虑使用 RT 介面代替。

编译及执行范例程式

范例档案位于 C:\Users\Public\Public Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\Motion_Sample，打开 Motion_Sample.sln 并编译之。

注意：档案总管 (File Explorer) 有两个路径：阶层路径与完整路径，阶层路径显示在地址栏中；完整路径显示在档案总管上方。右键点击 Motion_Sample.sln 后点选 Properties，将可看见位置 (Location) 为 C:\Users\Public\Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\Motion_Sample，此即为完整路径；而注意阶层路径为 Public Documents。若使用非英文的 Windows 系统，而需要复制贴上路径至地址栏中以加快查找范例速度，则请务必使用完整路径；若想要透过点击浏览范例档案夹，则请使用阶层路径；英文版 Windows 之档案夹将自动重新导向，因此就算贴上阶层路径，档案总管亦可引导至范例程式。

下图为范例程式的输出：

在 Visual Studio 中设定专案 properties

此范例为使用 Visual Studio 2019 中的 C++ 与 MFC 64 位元所开发之 MFC 应用程式，开发应用程式时，只要此应用程式为 64 位元，即可自行选择开发环境，因控制即时子系统需使用 64 位元。更多关于 Motion 范例的使用者介面之资讯，请见其 使用者介面。

创建应用程式时需在 Visual Studio 内修改以下属性：

1. 在 Solution Explorer 中的专案名称上点击右键，接著点选 Properties。
2. 进入 (Project name) Property Pages 对话框中的左窗格，点开 C/C++ 清单并点选 General。
3. 在右方区域的 Additional Include Directories 方框输入 "$(RTX64SDKDir4)include;$(KINGSTARSDKDir4)include;%(AdditionalIncludeDirectories)" （无空白鉴）。
4. 在左窗格中点开 Linker 清单后点选 General。
5. 右侧 Additional Library Directories 方框中，输入"$(KINGSTARSDKDir4)lib\amd64\;%(AdditionalLibraryDirectories)"（无空白键）。
6. 在左窗格中的 Linker 清单中，点选 Input。
7. 在右方区域的 Additional Dependencies 方框输入 "KsApi.lib;%(AdditionalDependencies)" （无空白鉴）。
8. 点击 Apply 后再按一下 OK。
9. 在专案的标头档（名称为 ProjectName.h）之 #endif // UNDER_RTSS 底下，输入以下代码：

#include <ksapi.h>
#include <ksmotion.h>

配置 KINGSTAR 子系统

使用 OnBnClickedInit 函式以开启 KINGSTAR 子系统，此函式在点击 Main menu 区域的 Init 时呼叫，其透过 GetSafeHwnd 函式（启动 window 的控制代码）获取参数 m_hWnd，之后将使用 _beginthread 函式建立一个新的执行绪，该执行绪为 InitThread 位址发送处，此 InitThread 函式负责所有初始化 KINGSTAR 子系统的工作，包括设定参数、功能、轴与 I/O 模组及连结。

范例使用的以下变数，其皆在 Motion_TestDlg.cpp 的开始即宣告。

• KsCommandStatus Command = { 0 } – KsCommandStatus 为包含函式状态的结构，可得知函式是否正在执行、完成、回传任何错误及何错误，我们使用 Command 以获取函式状态。
• KsError Code = errNoError – KsError 为包含 KINGSTAR 所有错误码的列举类型，errNoError 表示函式已成功。我们使用 Code 以接收函式的回传值，errNoError 表示函式已成功执行；若 Code 非 errNoError，则将呼叫 Destroy 而 KINGSTAR 子系统将终止。
• 旗标：通知程式条件已达到。
  • LinkInit – 讯号表示 EtherCAT 连结是否已建立。
  • LinkStart – 讯号表示 KINGSTAR 子系统是否已开启。
  • BThreadFlag – 讯号表示 KINGSTAR 处理序是否开始。
  • BCloseFlag – 讯号表示 KINGSTAR 处理序是否已被终结。
  • Edit_Flag – 讯号表示文字方块中的值已变更。
  • UIInit_Flag – 讯号表示在 EtherCAT 状态设为 Op 后，使用者介面使否准备好初始化。

初始化 KINGSTAR 子系统之连结

首先呼叫 Create，准备连接应用程式至 KINGSTAR 子系统，开始任何动作前，必须最先呼叫 Create。

Code = ::Create(0, 0);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to Create EtherCAT: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   return;
}
BThreadFlag = true;

设定 EtherCAT 循环时间

SetCycleTime设定 EtherCAT 循环时间，其时间单位为秒。欲使用低于 1 毫秒的循环时间，须备有高速计时器套件。注意非所有轴皆支援快速循环时间，若选择了不支援的循环时间，则每个轴的更新时间会自动且各自延长，欲使用快速循环时间，请确保电脑上的网卡可使用，只有具有低延迟的网卡才可支持快速循环。

Code = ::SetCycleTime(cycle1000);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to set SetCycleTime: %d\n"), Command.ErrorId);	
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

停用 RTX64 伺服主控台 (Server Console) 上的记录

EnableServerLog启用或停用 RTX64 Server Console 上的即时讯息，若将其停用，则主控台将只显示 KINGSTAR 讯息，因此我们选择将其启用。

Code = ::EnableServerLog(true);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableServerLog: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

设定存取模式

SetAxisAccessMode设定 EtherCAT 驱动器之资料传送模式，存取模式决定驱动器可用的控制模式，存取模式可在 KsAccessMode 列举类型中选择，预设之存取模式为 accessVelPos，

Code = ::SetAxisAccessMode(accessVelPos);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to SetAxisAccessMode: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

启用轴的数位输入及输出

EnableAxisInput和 EnableAxisOutput 启用或停用存取轴的数位输出及输入，首三个输入位元为负超程 (Overtravel)、正超程与原点复归感测器，若输入启用后可使用超程位元。

Code = ::EnableAxisInput(TRUE);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableAxisInput: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

Code = ::EnableAxisOutput(TRUE);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableAxisOutput: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

设定模拟轴的数量

SetConfiguredAxesCount设定模拟轴的数量，我们将数量设为一，若没有真实轴，将为 EtherCAT 网路建立一个模拟轴。

::SetConfiguredAxesCount(1);

启用实际速度

EnableActualVelocity读取轴的实际速度。

::EnableActualVelocity(true);

Motion 参数、单位转换及模拟 I/O 模组

在将范例连接到 KINGSTAR 子系统之前，我们使用以下函式来设定轴和 I/O 模组的运动参数和转换比率。

SetAxisMotionProfile：设定轴的运动轨迹。在此范例中，运动轨迹定义在 McProfileSettings 结构之 ProfileSettings 的实例中。

SetAxisCountsPerUnit：将使用者自定义位置单位的转换比率设定为轴使用的计数（脉冲）单位。在此范例中，Numerator 和 Denominator 为一，因此比率为 1:1，Reverse 为 false，因此轴的方向未倒转。

EnableAxisUnitConversion：启用轴使用真实世界单位。使用 SetAxisCountsPerUnit 设定转换比率后，需使用此函式来起动转换，此比率才会生效。

ConfigureIo：配置模拟 I/O 模组之设定。

SubsystemStatus：此结构用以获取 EtherCAT 连结的状态。我们宣告 SubsystemStatus KSMStatus 之实例，并使用 AxesCount 栏位获取 EtherCAT 网路上轴的数量。

SlaveStatus：此结构用来配置模拟 I/O 模组之设定。InputLength 与 OutputLength 为必填栏位，其决定了模拟 I/O 模组之输入及输出数量，其他于 SlaveStatus 中的栏位可留空，将会由预设值填入；若使用真实 I/O 模组，KINGSTAR 将自动侦测其输入与输出并填入所有栏位，我们宣告 SlaveStatus IoModuleStatus 之实例，以配置设定。

//Set up axes.
for (int i = 0; i < KSMStatus.AxesCount; i++)
{
   ::SetAxisMotionProfile(i, profileUnitPerSecond, ProfileSettings);
   ::SetAxisCountsPerUnit(i, Numerator, Denominator, Reverse);
   ::EnableAxisUnitConversion(i, TRUE);
}

//Set up I/O modules.
for (int i = 0; i < IOCOUNT; i++)
{
   SlaveStatus IoModuleStatus = { 0 };
   IoModuleStatus.InputLength = 16;
   IoModuleStatus.OutputLength = 16;
   ::ConfigureIo(i, IoModuleStatus);
}

开启 KINGSTAR 子系统

我们使用 Start 来开启 KINGSTAR 子系统和 EtherCAT 网路，当子系统无法启动造成程序永远在等待完成，我们为 Start 使用 WaitForCommand 将超时设置为 30 秒；若子系统开启，旗标 LinkInit 及 LinkStart 将设为 true；若 KINGSTAR 子系统无法启动，Start 将中止而 LinkInit 将设为 false。

在所有配置完成且 KINGSTAR 子系统开启后，UIInit_Flag 将设为 true。

Command = WaitForCommand(30, TRUE,::Start());
if (Command.Done)
{
   LinkInit = LinkStart = true;
}
else
{
   Str_Error.Format(_T("Failed to Start: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);

   LinkInit = false;
   return;
}
UIInit_Flag = true;

使用 KINGSTAR 子系统。

将真实装置连接到 KINGSTAR 子系统后，几个区域会显示装置的信息：MotionStatus、MasterStatus、SlaveStatus、ServoStatus、IOStatus，资讯将会依装置有所不同，可使用 Slave ID 以选择想要的装置，若无真实装置，则这些区域的大多数栏位将会是零。

轴

OnBnClickedServoOn 和 OnBnClickedServoOff 函式各别启用和停用轴。

Servo on

启用轴之前须检查一些状态，第一个为 EtherCAT 连接状态，若状态为 ecatOP，检查 EtherCAT 连结旗标 LinkStart，若为 true 则准备启用轴，启用之前使用 ResetAxis 来重置警报以防其中有错误，接著检查 SERVOCOUNT，若其大于零，则检查轴的 StatusWord 物件，第三位元（位元 2）应为 "操作启用 (operation enabled)"，若此位元为 true 则轴已启用。欲知第三位元是否为 true，使用位元 AND 运算子 (&)，条件 "if (ServoNoFlag(KSStatusWord[i]) != 1)" 如下描述：

1. 对选定轴的值和十六进制值 0x4 执行逻辑 AND 运算。
2. 取 AND 运算的结果并将值的位元向右移动两个位置。
3. 移动位元后，检查其是否等于一 (1)，若不是一，请执行以下代码。

若结果不等于一，代表轴未启动，因此我们使用 PowerAxis 来启动轴，在 PowerAxis 中，启动 (Enable) 参数为 TRUE。

void CMotion_TestDlg::OnBnClickedServoOn()
{
   if(KSMStatus.State == ecatOP)
   {
      if(LinkStart)
      {
         for(int i=0; i < KSMStatus.AxesCount; i++)
	     {	
	        ::ResetAxis(i);

            #if SERVOCOUNT > 0
	        if (ServoNoFlag(KSStatusWord[i]) != 1)
	        {
	           ::PowerAxis(i, TRUE, TRUE, TRUE);
	        }	
	        #endif
         }
      }	
   }
   else
   {
      MessageBox(_T("KSMStatus->EcatState != ecatOP"));	
   }
}

Servo off

欲停用轴，使用 PowerAxis，Enable 为 FALSE。

void CMotion_TestDlg::OnBnClickedServoOff()
{
   for (int i = 0; i < KSMStatus.AxesCount; i++)
      ::PowerAxis(i, FALSE, TRUE, TRUE);
}

重设警报

OnBnClickedResetalarm 函式重设警报，轴执行时可能会有错误发生，当错误发生时警报即出现，因此我们使用 ResetAxis 来重设轴的警报。

void CMotion_TestDlg::OnBnClickedResetalarm()
{
   for (int i = 0; i < KSMStatus.AxesCount; i++)
   {
      ::ResetAxis(i);
   }
}

Motion 控制

Jog 和 Move 为测试轴的基本运动，Jog 在不指定目标位置情况下测试速度；Move 在有目标位置情况下测试移动，而 Home 使轴回归原点。

寸动 (Jog)

Jog 在给定的方向使用 T 曲线加速度，变数 UJog_Pulse 储存寸动之速度，其预设值为 10000，m_AxisNumber.GetCurSel() 获取轴之索引，JogAxis 执行寸动运动，移动方向可为向前或向后，+JOG 使用函式 OnBnClickedPjog 以向前移动；而 -JOG 使用 OnBnClickedNjog 以向后移动。

void CMotion_TestDlg::OnBnClickedPjog()
{
   ::JogAxis(m_AxisNumber.GetCurSel(), double(UJog_Pulse), 36000, 36000, 360000, mcPositiveDirection);
}
void CMotion_TestDlg::OnBnClickedNjog()
{
   ::JogAxis(m_AxisNumber.GetCurSel(), double(UJog_Pulse), 36000, 36000, 360000, mcNegativeDirection);
}

使用 HaltAxis 以停止寸动运动。

void CMotion_TestDlg::OnBnClickedJogstop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

当输入寸动速度的新值，UJog_Pulse 将更新，UpdateData 在 UI 上读取更新的速度，资料更改后，旗标 Edit_Flag 将设为 true。

void CMotion_TestDlg::OnEnChangeJogPulse()
{
   Edit_Flag = false;
   UpdateData(true);
   Edit_Flag = true;
}

移动 (Move)

Move 使用运动曲线以建立 S 曲线轨迹至目的地，与 Jog 相似，变数 UMove_Pulse 储存移动的距离，其预设值为 10000，m_AxisNumber.GetCurSel() 获取轴之索引。其中有两个 MoveAxis 函式：MoveAxisAbsolute 与 MoveAxisRelative，此范例使用 MoveAxisRelative，移动方向可为向前或向后，+MOVE 使用函式 OnBnClickedPmove 以向前移动；而 -MOVE 使用 OnBnClickedNmove 以向后移动。

void CMotion_TestDlg::OnBnClickedPmove()
{
   ::MoveAxisRelative(m_AxisNumber.GetCurSel(), double(UMove_Pulse), 3600, 36000, 36000, 3600000, mcAborting);
}
void CMotion_TestDlg::OnBnClickedNmove()
{
   ::MoveAxisRelative(m_AxisNumber.GetCurSel(), double(0-UMove_Pulse), 3600, 36000, 36000, 3600000, mcAborting);
}

使用 HaltAxis 以停止移动。

void CMotion_TestDlg::OnBnClickedMovestop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

当输入 MoveAxisRelative 之距离的新值，UJog_Pulse 将更新，UpdateData 在 UI 上读取更新的距离，资料更改后，旗标 Edit_Flag 将设为 true。

void CMotion_TestDlg::OnEnChangeMovePulse()
{
   Edit_Flag = false;
   UpdateData(true);
   Edit_Flag = true;
}

原点复归 (Home)

Home 将轴移动到已知位置，其中原点复归过程写在函式 OnBnClickedHome 中。欲将轴复归原点，先使用 SetAxisParameter 以设定原点复归模式，在此范例使用 mcSlaveHomingMode，其为伺服驱动器所提供的原点复归方法，接著，使用 HomeAxis 以将轴回归原点，而 m_AxisNumber.GetCurSel() 获取轴之索引。

void CMotion_TestDlg::OnBnClickedHome()
{
   //There is test the slave homing.
   ::SetAxisParameter(m_AxisNumber.GetCurSel(), mcSlaveHomingMode, 33, mcImmediately);
   Command = WaitForCommand(30, TRUE, ::HomeAxis(m_AxisNumber.GetCurSel(), 0, HomeVel, 
           HomeEndVel, HomeAcc, HomeDec, HomeJerk, mcPositiveDirection, homingSlave));
   if (!Command.Done)
   {
      Str_Error.Format(_T("Failed to Home: %d\n"), Command.ErrorId);
      GetDlgItem(IDC_ERROR_RETURN)->SetWindowText(Str_Error);
   }
}

使用 HaltAxis 以停用原点复归。

void CMotion_TestDlg::OnBnClickedHomestop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

获取状态

当硬体连接时，我们可以透过某些变数和函式来检查它的状态。

索引

在此范例中宣告了五个变数来接收装置的索引：

• m_AxisNumber：轴的索引（伺服马达），其在运动控制 (Motion control) 区域中用作伺服索引 (Servo Index)，请见 使用者介面。
• m_ServoNumber：轴的索引，其在伺服器状态 (ServoStatus) 区域中用作伺服索引 (Servo Index)。
• m_MotionNumber：轴的索引，其在运动状态 (MotionStatus) 区域中用作伺服索引 (Servo Index)。
• m_SlaveNumber： EtherCAT 从站的索引，其在从站状态 (SlaveStatus ) 区域中用作从站 ID (Slave ID)。
• m_IONumber： I/O 模组的索引，其在 IOStatus 区域中用作 I/O 索引 (I/O Index)。

EtherCAT 主站之状态

使用 GetStatus 以获取 EtherCAT 主站状态。

GetStatus(&KSMStatus, KSSubsystemDiagnostics);

EtherCAT 从站之状态

使用 GetSlaveById 以获取 EtherCAT 从站状态。

::GetSlaveById(i, &KSSlaveStatus[i]);

轴状态

使用 GetAxisMotionState 以获取轴（伺服驱动器）之运动状态。

在 SlaveStatus 与 ServoStatus 区域中，使用 GetAxisByIndex 以获取轴的资讯与编码器解析度。

在 ServoStatus 区域中，使用 ReadAxisStatusWord 以获取轴之 CANopen (CiA 402) 状态（从 StatusWord 至 LimitActive）。

在 ServoStatus 区域中，使用函式： GetAxisVelocity, GetAxisPosition, ReadAxisInputs, ReadAxisOutputs 以获取 Velocity, Position, Servo DI, Servo DO 之状态。

在 ServoStatus 区域中，使用变数 Read_HomingStatus 以获取轴的原点复归状态 (Homing Status)。一般来说，伺服驱动器的数位输入之前三个位元（位元 0, 1, 2） 为负超程 (Negative Overtravel)、正超程 (Positive Overtravel) 及原点感测器 (Home Sensor)，但位元可能因伺服驱动器而有所不同，在此范例中我们检查位元 0 或位元 1 和位元 2 是否为 TRUE （位元 1 和 2 一起检查），并将结果分配给 Read_HomingStatus。

while(BCloseFlag)
{
   while (BThreadFlag)
   {
      ::GetStatus(&KSMStatus, KSSubsystemDiagnostics);
      for(int i = 0; i < KSMStatus.AxesCount; i++)
      {
         ::GetAxisMotionState(i, MotionCXY[i].KSMcMotionState, MotionCXY[i].KSMcDirection);
         ::GetAxisByIndex(i, &KSAxisStatus[i], &AXIS_RESOLUTION[i], &AXIS_DI[i], &AXIS_DO[i]);
         ::ReadAxisStatusWord(i, &KSStatusWord[i]);   //Read StatusWord 
         ::ReadAxisActualVelocity(i, MotionCXY[i].VelocityValue);   //Get Velocity status
         ::ReadAxisActualPosition(i, MotionCXY[i].PositionValue);   //Get PositionValue status
         SERVOIO_DI_ERROR[i] = ::ReadAxisInputs(i, &SERVOIO_DI[i]);   //Get ServoDI
         SERVOIO_DO_ERROR[i] = ::ReadAxisOutputs(i, &SERVOIO_DO[i]);   //Get ServoDO	
         Read_HomingStatus[i] = HomingStatus(KSStatusWord[i]);
      }
   .........
}

I/O 模组之状态

使用 ReadInputWord 和 ReadOutputWord 获取 I/O 模组状态，IO_DI 与 IO_DO 接收读取的 WORD 值。

for(int i = 0; i < KSMStatus.IOCount; i++)
{
   #if IOCOUNT > 0
   ::ReadInputWord(i, 0, &IO_DI[i]);
   ::ReadOutputWord(i, 0, &IO_DO[i]);
   #endif
}

停止 KINGSTAR 子系统

在停止 KINGSTAR 子系统前须先停止所有轴，可用 HaltAxis 以停止轴。

KINGSTAR 子系统必须在关闭应用程式前停止，若电脑未预期关闭，所有侦测到主站遗失的 EtherCAT 从站都将被置放到安全警告状态，子系统会在下一次开启时尝试重置从站，然而有些从站在重启后可能不会正常运作，若此情况发生，请关闭、重启或重置这些从站，欲重置从站，使用 ResetAxis 函式或重启从站。

关闭连结

欲确保 KINGSTAR 子系统已停止，添加了关闭逻辑至 OnBnClickedExit 函式，首先设定旗标 BCloseFlag 和 BThreadFlag 为 false，接著使用 Stop 以停止 EtherCAT 连结和使用 Destroy 终止 KINGSTAR 子系统处理序，

void CMotion_TestDlg::OnBnClickedExit()
{
   BCloseFlag = BThreadFlag = false;
   ::Stop();
   ::Destroy();
   CDialogEx::OnOK();
}

若有多个应用程式在使用 KINGSTAR，请记住此任一应用程式在呼叫 Destroy 函式前不可呼叫 KINGSTAR 函式。